﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Graphics.EffectObjects;
using Drifting.MotionPlanning.Primitives;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Drifting.Foundation;
using Drifting.MotionPlanning.Systems;

namespace Drifting.Graphics.Visualizers
{
    public class Skin_Drift8D : Skin_G
    {
        public Skin_Drift8D(List<State> train, GraphicsDevice device)
            : base(train, device)
        {
            max_point = 100000;
        }

        override public bool TranslateStateForVisualization(State x, ref float[] q_tmp, ref Color colorOfCost, double costBound)
        {
            if (x == null) return false;
            double[] q = x.Q;
            if (q == null) return false;

            double cost = x.Cost;
            double normalizedCost = 1 - cost / costBound;
            double complementaryNormalizedColor = 1 - normalizedCost;

            Color lowColor = Color.Gold;
            Color highColor = Color.Blue;
            colorOfCost.R = (byte)(normalizedCost * lowColor.R + complementaryNormalizedColor * highColor.R);
            colorOfCost.G = (byte)(normalizedCost * lowColor.G + complementaryNormalizedColor * highColor.G);
            colorOfCost.B = (byte)(normalizedCost * lowColor.B + complementaryNormalizedColor * highColor.B);

            q_tmp[0] = (float)q[0];
            q_tmp[1] = (float)Drift8D.GetAOA(q);
            q_tmp[2] = (float)-q[1];

            return true;
        }
    }
}
